#ifndef TRACKING_H
#define TRACKING_H

#include "Viewer.h"
#include "MapDrawer.h"
#include "Localoptim.h"
#include "PoseFrame.h"
#include "System.h"
#include "ImuTypes.h"
#include "Filter/ESKF.h"
#include "Converter.h"
#include "Optimizer.h"

#include "nav_msgs/Odometry.h"

namespace msf
{
class Viewer;
class MapDrawer;
class Atlas;
class LocalOptimizing;
class System;
class Settings;

class Tracking
{
public:
      EIGEN_MAKE_ALIGNED_OPERATOR_NEW
      Tracking(System* pSys, MapDrawer* pMapDrawer, Atlas* pAtlas, const int sensor, Settings* settings);

      ~Tracking();

      // 预处理输入并调用Track().
      Sophus::SE3f GrabOdom(const nav_msgs::OdometryConstPtr &odom, const double &timestamp);

      void GrabImuData(const IMU::Point &imuMeasurement);

      void SetLocalOptimizer(LocalOptimizing* pLocalOptimizer);
      void SetViewer(Viewer* pViewer);

      // Use this function if you have deactivated local optimizing and you only want to localize the odom.
      void InformOnlyTracking(const bool &flag);

      void UpdatePoseFrameIMU(const float s, const IMU::Bias &b, KeyPoseFrame* pCurrentKeyPoseFrame);
      KeyPoseFrame* GetLastKeyPoseFrame()
      {
            return mpLastKeyPoseFrame;
      }

      void CreateMapInAtlas();
public:

      //跟踪状态
      enum eTrackingState{
            SYSTEM_NOT_READY=-1,
            NO_ODOM_YET=0,
            NOT_INITIALIZED=1,
            OK=2,    
            LOST=4,
      };

      eTrackingState mState;
      eTrackingState mLastProcessedState;

      // Input sensor
      int mSensor;

      // Current PoseFrame
      PoseFrame mCurrentPoseFrame;
      PoseFrame mLastPoseFrame;

      nav_msgs::Odometry mOdom;

      //用于恢复全部相机轨迹的列表
      list<Sophus::SE3f> mlRelativeFramePoses;
      list<KeyPoseFrame*> mlpReferences;
      list<double> mlFrameTimes;
      list<bool> mlbLost;

      // 具有估计位姿的帧
      int mTrackedPFr;
      bool mbStep;

      // 如果不进行LocalOptimize则为True,我们只进行定位
      bool mbOnlyTracking;

      void Reset(bool bLocOptim = false);
      void ResetActiveMap(bool bLocMap = false);

      float mMeanTrack;
      bool mbInitWith3KFs;
      double t0; // time-stamp of first read frame
      double t0odom; // time-stamp of first inserted keyposeframe
      double t0IMU; // time-stamp of IMU initialization
      bool mFastInit = false;

protected:

      // 主线程跟踪函数.独立于输入传感器.
      void Track();

      // 初始化
      void OdomInitialization();

      void CheckReplacedInLastPoseFrame();
      bool TrackReferenceKeyPoseFrame();
      void UpdateLastPoseFrame();
      bool TrackWithMotionModel();
      bool PredictStateIMU();
      bool TrackWithESKF();

      void UpdateLocalOptim();

      void UpdateLocalKeyPoseFrames();

      bool TrackLocalOptim();

      bool NeedNewKeyPoseFrame();
      void CreateNewKeyPoseFrame();

      // 求解从上一帧开始的预积分
      void PreintegrateIMU();

      // 重置IMU偏置和计算帧速度
      void ResetFrameIMU();

      bool mbMapUpdated;

      // 从上一帧开始的预积分 
      IMU::Preintegrated *mpImuPreintegratedFromLastKPF;

      // 帧间IMU测量值队列
      std::list<IMU::Point> mlQueueImuData;

      // 从前一帧到到当前帧IMU测量值vector
      std::vector<IMU::Point> mvImuFromLastPoseFrame;
      std::mutex mMutexImuQueue;

      // Imu校准参数
      IMU::Calib *mpImuCalib;

      // 过去的偏差估计
      IMU::Bias mLastBias;

      // 单纯定位，
      //bool mbIO;

      // 其他的线程指针
      LocalOptimizing* mpLocalOptimizer; 

      // Initialization
      bool mbReadyToInitialize;

      // Local Optimize
      KeyPoseFrame* mpReferenceKPF;
      std::vector<KeyPoseFrame*> mvpLocalKeyPoseFrames;

      // System
      System* mpSystem;

      // Drawers
      Viewer* mpViewer;
      MapDrawer* mpMapDrawer;
      bool bStepByStep;

      //Atlas
      Atlas* mpAtlas;

      float mImuFreq;
      double mImuPer;
      bool mInsertKPFsLost;

      // 新的关键位姿帧规则
      int mMinPoseFrames;
      int mMaxPoseFrames;

      int mnFirstImuFrameId;
      int mnFramesToResetIMU;

      // 上一PoseFrame,KeyPoseFrame信息
      KeyPoseFrame* mpLastKeyPoseFrame;
      unsigned int mnLastKeyPoseFrameId;

      unsigned int mnFirstPoseFrameId;
      unsigned int mnInitialPoseFrameId;
      unsigned int mnLastInitPoseFrameId;
      
      bool mbCreatedMap;

      //Motion Model
      bool mbVelocity{false};
      Sophus::SE3f mVelocity;

      void newParameterLoader(Settings* settings);

      //ESKF
      ESKFPtr eskf_ptr_;
};
} //namespase msf

#endif // TRACKING_H